#include <Wire.h>
#include <BMP085.h>

void setup()
{
  Serial.begin(9600);
  Wire.begin();
}

void loop()
{
}

// ------------------------------------------------------- //
// - Altitude Control Code
// ------------------------------------------------------- //

#define SMOOTHING_AMOUNT 0.7f
#define UPDATE_INTERVAL 200  // milliseconds

BMP085 pressure_sensor = BMP085();
long smoothed_altitude = 0;
unsigned long last_update = 0;
byte motor_power = 0;
long accum_error = 0;
long last_altitude = 0;
float pgain = 1;
float igain = 1;
float dgain = 1;

// get the motor speed using a PID on the altitude
byte vertical_motor_control(long target_altitude)
{
  if (millis() < last_update)
    last_update = 0;
  if ((millis() - last_update) > UPDATE_INTERVAL)
  {
    last_update += UPDATE_INTERVAL;
    float power = update_pid(target_altitude, get_altitude());
    if (power < -255)
      power = -255;
    if (power > 255)
      power = 255;
    motor_power = (byte)power;
  }
  
  return motor_power;
}

float update_pid(long target, long current)
{
  // find current error
  // the P term comes from current error
  long error = target - current;
  float pterm = pgain * error;
  
  // find the I term from accumulated error
  accum_error += error;
  float iterm = igain * accum_error;
  
  // find the D term from differential changes in readings (this is very sensitive to noise)
  float dterm = dgain * (current - last_altitude);
  last_altitude = current;
  
  // the amount of feedback is the sum of all terms
  return pterm + iterm - dterm;
}

// call in the setup() function
void init_pressure_sensor(float p, float i, float d)
{
  pgain = p;
  igain = i;
  dgain = d;
  
  pressure_sensor.init(MODE_ULTRA_HIGHRES, 0, true);
}

// get the current altitude, filtered to smooth out noise
long get_altitude()
{
  long altitude = 0;
  pressure_sensor.getAltitude(&altitude);
  smoothed_altitude = smooth(altitude, SMOOTHING_AMOUNT, smoothed_altitude);
  
  return smoothed_altitude;
}

// simple lowpass filter
long smooth(long data, float filter, long smoothed)
{
  return (long)((data * (1 - filter)) + (smoothed * filter));
}
